/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 *
 * @author Eric
 */
public class MecanumDrive {
    
    Joystick joy1 = new Joystick(1);
    Joystick joy2 = new Joystick(2);
    static Jaguar frontleft;
    static Jaguar frontright;               
    static Jaguar backleft;
    static Jaguar backright;
    
    ADXL345_I2C accel;
    RobotDrive drive;
    Gyro drivegyro;
    
    boolean fieldframeinuse=false;
    
    
    public MecanumDrive(int fleft,int rleft,int fright,int rright,int robogyro)
    {
        frontleft = new Jaguar(fleft);
        frontright = new Jaguar(fright);
        backleft = new Jaguar(rleft);
        backright = new Jaguar(rright);
    
        accel = new ADXL345_I2C(1,ADXL345_I2C.DataFormat_Range.k8G);
        drive = new RobotDrive(frontleft,backleft,frontright,backright);
        drivegyro=new Gyro(robogyro);
        
      
    }
    
    void resetref()
    {
        drivegyro.reset();
    }
    
    void driveFieldFrame()
    {
        fieldframeinuse=true;
        drive.mecanumDrive_Cartesian(joy1.getX(),joy1.getY(),joy2.getX(),drivegyro.getAngle());
    }
    
    void driveBodyFrame()
    {
        fieldframeinuse=false;
        drive.mecanumDrive_Polar(joy1.getY(),joy1.getX(),joy2.getX());
    }
    
    void driverStationOutputs()
    {
        SmartDashboard.putBoolean("Field Frame:", fieldframeinuse);
        SmartDashboard.putNumber("Gyro Angle", drivegyro.getAngle());
        
        SmartDashboard.putNumber("Front Left",frontleft.get());
        SmartDashboard.putNumber("Front Right",frontright.get());
        SmartDashboard.putNumber("Back Left",backleft.get());
        SmartDashboard.putNumber("Back Right",backright.get());
    }
}
